package com.googlecode.curvedmetromaps.core.geometry;

import java.util.HashSet;

import org.jbox2d.common.Vec2;

public class BezierUtil {
	// pre-computed z(i,j) for cubic Bézier curves.
	private static final float[][]	Z_CUBIC		= { { 1.0f, 0.6f, 0.3f, 0.1f }, { 0.4f, 0.6f, 0.6f, 0.4f },
			{ 0.1f, 0.3f, 0.6f, 1.0f }			};
	// pre-computed z(i,j) for quadratic Bézier curves.
	private static final float[][]	Z_QUAD		= { { 1.0f, 2f / 3f, 1f / 3f }, { 1f / 3f, 2f / 3f, 1.0f } };
	// maximum recursion depth
	private static final int		MAX_DEPTH	= 8;
	// flatness tolerance
	private static final float		EPSILON		= (float) Math.pow(2., -MAX_DEPTH - 1);

	/**
	 * subdivide( _c:Array, _t:Number, _left:Array, _right:Array ) - deCasteljau
	 * subdivision of an arbitrary-order Bezier curve
	 * @param _c :Array array of control points for the Bezier curve
	 * @param _t :Number t-parameter at which the curve is subdivided (must be
	 *            in (0,1) = no check at this point
	 * @param _left :Array reference to an array in which the control points, of
	 *            the left control cage after subdivision are stored
	 * @param _right :Array reference to an array in which the control points,
	 *            of the right control cage after subdivision are stored
	 */
	public static void subdivide(Vec2[] _c, float _t, Vec2[] _left, Vec2[] _right) {
		int n = _c.length;
		int degree = n - 1;
		Vec2[][] p = new Vec2[n][n];
		System.arraycopy(_c, 0, p[0], 0, n);
		float t1 = 1.0f - _t;

		for (int i = 1; i <= degree; i++) {
			for (int j = 0; j <= degree - i; j++) {
				Vec2 pa = p[i - 1][j].mul(t1);
				Vec2 pb = p[i - 1][j + 1].mul(_t);
				p[i][j] = pa.addLocal(pb);
			}
		}

		if (_left != null) {
			for (int j = 0; j <= degree; j++) {
				_left[j] = p[j][0];
			}
		}

		if (_right != null) {
			for (int j = 0; j <= degree; j++) {
				_right[j] = p[degree - j][j];
			}
		}
	}

	/**
	 * Projects a point on a Bézier curve.
	 * @param _p point to project
	 * @param v Control points of Bézier curve.
	 */
	public static Vec2 pointProjection(Vec2 _p, Vec2[] v) {
		// degree of input Bézier curve
		int n = 2;

		// Instead of power form, convert the function whose zeros are required
		// to Bézier form
		Vec2[] w = toBezierForm(_p, v);

		// Find roots of the Bézier curve with control points stored in 'w'
		// (algorithm is recursive, this is root depth of 0)
		HashSet<Float> roots = findRoots(w, 2 * n - 1, 0);
		roots.add(0f);
		roots.add(1f);

		// Find root closest to _p
		float dMin = Float.POSITIVE_INFINITY;
		Vec2 pMin = null;
		for (Float T : roots) {
			float t = T.floatValue();
			if (t >= 0 && t <= 1) {
				Vec2 p = interpolate(t, v);
				float d = p.sub(_p).lengthSquared();

				if (d < dMin) {
					dMin = d;
					pMin = p;
				}
			}
		}

		return pMin;
	}

	public static double curvature(Vec2[] v) {
		Vec2 d12 = v[2].sub(v[1]);
		double h = BasicGeometry.pointLineDistance(v[0], v[2], v[1]);
		return h / (2. * (Vec2.dot(d12, d12)));
	}

	public static Vec2 interpolate(float t, Vec2[] v) {
		if (v.length == 3) {
			return new Vec2((1f - t) * (1f - t) * v[0].x + 2f * (1 - t) * t * v[1].x + t * t * v[2].x, (1f - t)
					* (1f - t) * v[0].y + 2f * (1 - t) * t * v[1].y + t * t * v[2].y);
		} else {
			return null;
		}
	}

	/**
	 * Compute control points of the polynomial resulting from the inner product
	 * of B(t)-P and B'(t), constructing the result as a Bézier curve of order
	 * 2n-1, where n is the degree of B(t).
	 */
	private static Vec2[] toBezierForm(Vec2 _p, Vec2[] _v) {
		int n = _v.length - 1;
		int degree = 2 * n - 1;

		Vec2[] c = new Vec2[n + 1];
		Vec2[] d = new Vec2[n];
		Vec2[] w = new Vec2[degree + 1];

		float pX = _p.x;
		float pY = _p.y;

		for (int i = 0; i <= n; i++) {
			c[i] = _v[i].sub(_p);
		}

		for (int i = 0; i <= n - 1; i++) {
			d[i] = _v[i + 1].sub(_v[i]).mulLocal(n);
		}

		// inner product table

		float[][] cd = new float[n][n + 1];
		for (int row = 0; row <= n - 1; row++) {
			for (int col = 0; col <= n; col++) {
				cd[row][col] = Vec2.dot(d[row], c[col]);
			}
		}

		// Bézier is uniform parameterized
		for (int i = 0; i <= degree; i++) {
			w[i] = new Vec2(i / (float) degree, 0);
		}

		// reference to appropriate pre-computed coefficients
		float[][] z = (n == 3 ? Z_CUBIC : Z_QUAD);

		// accumulate y-coords of the control points along the skew diagonal of
		// the (n-1) x n matrix of c.d and z values
		int m = n - 1;
		for (int k = 0; k <= n + m; k++) {
			int lb = Math.max(0, k - m);
			int ub = Math.min(k, n);
			for (int i = lb; i <= ub; i++) {
				int j = k - i;
				w[i + j].y += cd[j][i] * z[j][i];
			}
		}

		return w;
	}

	/**
	 * Return roots in [0,1] of a polynomial in Bernstein-Bézier form.
	 */
	private static HashSet<Float> findRoots(Vec2[] _w, int _degree, int _depth) {
		// t-values of roots
		HashSet<Float> t = new HashSet<Float>();
		//int m = (2 * _degree) - 1;

		switch (crossingCount(_w, _degree)) {
		case 0:
			return new HashSet<Float>();
		case 1:
			// Unique solution - stop recursion when the tree is deep enough
			// (return 1 solution at midpoint)
			if (_depth >= MAX_DEPTH) {
				t.add(0.5f * (_w[0].x + _w[_degree].x));
				return t;
			} else if (isControlPolygonLinear(_w, _degree)) {
				t.add(computeXIntercept(_w, _degree));
				return t;
			}
			break;
		}

		// Otherwise, solve recursively after subdividing control polygon
		Vec2[] left = new Vec2[_w.length];
		Vec2[] right = new Vec2[_w.length];
		subdivide(_w, 0.5f, left, right);
		HashSet<Float> leftT = findRoots(left, _degree, _depth + 1);
		HashSet<Float> rightT = findRoots(right, _degree, _depth + 1);

		// Gather solutions
		t.addAll(leftT);
		t.addAll(rightT);
		return t;
	}

	private static int crossingCount(Vec2[] _v, int _degree) {
		int nCrossings = 0;
		int sign = _v[0].y < 0 ? -1 : 1;
		int oldSign = sign;
		for (int i = 1; i <= _degree; ++i) {
			sign = _v[i].y < 0 ? -1 : 1;
			if (sign != oldSign)
				nCrossings++;

			oldSign = sign;
		}

		return nCrossings;
	}

	/*
	 * Is the control polygon for a Bézier curve suitably linear for subdivision
	 * to terminate?
	 */
	private static boolean isControlPolygonLinear(Vec2[] _v, int _degree) {
		// Given array of control points, _v, find the distance from each
		// interior control point to line connecting v[0] and v[degree]

		// implicit equation for line connecting first and last control points
		float a = _v[0].y - _v[_degree].y;
		float b = _v[_degree].x - _v[0].x;
		float c = _v[0].x * _v[_degree].y - _v[_degree].x * _v[0].y;

		float abSquared = a * a + b * b;
		float[] distance = new float[_degree]; // Distances from control points
												// to line

		for (int i = 1; i < _degree; ++i) {
			// Compute distance from each of the points to that line
			distance[i] = a * _v[i].x + b * _v[i].y + c;
			if (distance[i] > 0.0) {
				distance[i] = (distance[i] * distance[i]) / abSquared;
			}
			if (distance[i] < 0.0) {
				distance[i] = -((distance[i] * distance[i]) / abSquared);
			}
		}

		// Find the largest distance
		float maxDistanceAbove = 0.0f;
		float maxDistanceBelow = 0.0f;
		for (int i = 1; i < _degree; ++i) {
			if (distance[i] < 0.0) {
				maxDistanceBelow = Math.min(maxDistanceBelow, distance[i]);
			}
			if (distance[i] > 0.0) {
				maxDistanceAbove = Math.max(maxDistanceAbove, distance[i]);
			}
		}

		// Implicit equation for zero line
		float a1 = 0.0f;
		float b1 = 1.0f;
		float c1 = 0.0f;

		// Implicit equation for "above" line
		float a2 = a;
		float b2 = b;
		float c2 = c + maxDistanceAbove;

		float det = a1 * b2 - a2 * b1;
		float dInv = 1.0f / det;

		float intercept1 = (b1 * c2 - b2 * c1) * dInv;

		// Implicit equation for "below" line
		a2 = a;
		b2 = b;
		c2 = c + maxDistanceBelow;

		float intercept2 = (b1 * c2 - b2 * c1) * dInv;

		// Compute intercepts of bounding box
		float leftIntercept = Math.min(intercept1, intercept2);
		float rightIntercept = Math.max(intercept1, intercept2);

		float error = 0.5f * (rightIntercept - leftIntercept);

		return error < EPSILON;
	}

	/*
	 * Compute intersection of line segnet from first to last control point with
	 * horizontal axis
	 */
	private static float computeXIntercept(Vec2[] _v, int _degree) {
		float XNM = _v[_degree].x - _v[0].x;
		float YNM = _v[_degree].y - _v[0].y;
		float XMK = _v[0].x;
		float YMK = _v[0].y;

		float detInv = -1.0f / YNM;

		return (XNM * YMK - YNM * XMK) * detInv;
	}

}
